#include "InputFeedBackLoop.h"


//extern volatile unsigned long timer0_overflow_count; 
// constructor
InputFeedbackLoop::InputFeedbackLoop(uint8_t newInputPin, float newInputDampener, float newFeedbackMagnifier, float newCalibrationAdjustMaximum, float newDegreeTransitionConstant){
	inputPin = newInputPin;
	inputDampener = newInputDampener;
	feedbackMagnifier = newFeedbackMagnifier;
	calibrationAdjustMaximum = newCalibrationAdjustMaximum;
	degreeTransitionConstant = newDegreeTransitionConstant;
	//bufferSize = newBufferSize;
	reset();
}
// end Constructor

//private functions

void InputFeedbackLoop::reset(){
	
	//other
	sumOfBuffer = 0;
	countOfBuffer = 0;  
	currentBufferAverage = 0;
	previousBufferAverage = 0;
	calibration = 0;
	calibrationAdjust = 0;
	lastInput = 0;
	currentInputAwayFromCalibration = 0;
	currentDegreeOutputFrom90 = 0;    
	firstIntegrationSinceStart = 0;
	secondIntegrationSinceStart = 0;
	
	previousTime = milisecondsSinceOn();
	currentTime = milisecondsSinceOn();
	deltaTime = 0;
	
	//    bufferedInput = new int [1000]();
	for (int index = 0; index < bufferSize; index++) {
		bufferedInput[index] = 0;
	}
}

unsigned long InputFeedbackLoop::milisecondsSinceOn(){
	return millis();//(((timer0_overflow_count << 8) + TCNT0) * 4);
}

uint16_t InputFeedbackLoop::getPinInput(){
	lastInput = analogRead(inputPin);
	return  lastInput;
}

void InputFeedbackLoop::readNewInput(){
	previousBufferAverage = currentBufferAverage;
	sumOfBuffer -= bufferedInput[countOfBuffer];
	bufferedInput[countOfBuffer] = getPinInput();
	sumOfBuffer += bufferedInput[countOfBuffer];
	currentBufferAverage = sumOfBuffer / bufferSize;
	countOfBuffer ++;
	if(countOfBuffer == bufferSize){
		countOfBuffer = 0;
	}
}

void InputFeedbackLoop::calculateActualDegreeChanges() {
	currentInputAwayFromCalibration = ((currentBufferAverage - calibration) / inputDampener);
	currentDegreeOutputFrom90 = 90 + (currentInputAwayFromCalibration * feedbackMagnifier);
        if(currentDegreeOutputFrom90 < 1) 
          currentDegreeOutputFrom90 = 1;
        if(currentDegreeOutputFrom90 > 179)
          currentDegreeOutputFrom90 = 179;
}

void InputFeedbackLoop::adjustChangeSinceStart(){
	deltaTime = currentTime - previousTime;
	if(deltaTime > 0){
	float deltaTimeFractionOfSecond = ((float)deltaTime) / 1000.0;
	//currentInputAwayFromCalibration is of units where 98 = 1g, or 98 = 9.8m/s^s or 10 = 1.0m/s^2,
	// so divide by 10 to get to root meters.
	float metersPerSecondPerSecond = ((float)currentInputAwayFromCalibration) / 10.0;
	firstIntegrationSinceStart += (metersPerSecondPerSecond * deltaTimeFractionOfSecond * 500);
        if(firstIntegrationSinceStart > .3){
          firstIntegrationSinceStart -= .03;
        }else if(firstIntegrationSinceStart < -.3){
          firstIntegrationSinceStart +=.03; 
        }else{
          firstIntegrationSinceStart *= .99;
        }
        //firstIntegrationSinceStart -= firstIntegrationSinceStart * .05;
	//second is resulting meters moved due to acceleration felt.
	secondIntegrationSinceStart += (firstIntegrationSinceStart * deltaTimeFractionOfSecond);
	}
	previousTime = currentTime;
	currentTime = milisecondsSinceOn();
}


//end private functions

//public functions
void InputFeedbackLoop::Calibrate(){
	InitializeBuffer();
	calibration = currentBufferAverage;
}

void InputFeedbackLoop::InitializeBuffer(){
	for (int index = 0; index <= bufferSize; index++) {
		readNewInput();
	}
}

int InputFeedbackLoop::LastInput(){
	return lastInput;
}

int InputFeedbackLoop::ReadAndAverageOverBuffer(){
	readNewInput();
	calculateActualDegreeChanges();
	adjustChangeSinceStart();
	return currentDegreeOutputFrom90;
}

int InputFeedbackLoop::getDegrees(){
	return currentDegreeOutputFrom90;
}

float InputFeedbackLoop::getFirstIntegrationSinceStart(){
	return firstIntegrationSinceStart;
}

float InputFeedbackLoop::getSecondIntegrationSinceStart(){
	return secondIntegrationSinceStart;
}

int InputFeedbackLoop::Calibration(){
	return calibration;
}

int InputFeedbackLoop::CurrentBufferAverage(){
	return currentBufferAverage;
}

int InputFeedbackLoop::CurrentInputAwayFromCalibration(){
	return currentInputAwayFromCalibration;
}
//end public functions
